#include <iostream>
#include <opencv2/core.hpp>
#include <opencv2/imgcodecs.hpp>
#include <opencv2/imgproc.hpp>
#include <opencv2/highgui.hpp>

typedef struct ContourInformation {
    std::vector<cv::Point> contour;
    int NBD;
    bool is_hole_border;
    int nxt_contour, prev_contour, first_child_contour, parent_contour;
} ContourInformation;

static void _find_contours(const cv::Mat& src, std::vector<std::vector<cv::Point>>& contours, std::vector<cv::Vec4i>& hierarchy) {
    cv::Mat image;
    cv::copyMakeBorder(src, image, 1, 1, 1, 1, cv::BORDER_CONSTANT | cv::BORDER_ISOLATED, cv::Scalar(0));
    cv::threshold(image, image, 0, 1, cv::THRESH_BINARY);
    image.convertTo(image, CV_32SC1);
    
    int NBD = 1;
    int LNBD = 1;
    int width = image.cols-1;
    int height = image.rows-1;
    int i, j, i1, j1, i2, j2, i3, j3, i4, j4;
    int cur_pixel, prev_pixel, nxt_pixel, tmp_pixel;
    bool is_hole_border;
    int c;
    
    int neighbor8_offset_x[] = {1, 1, 0, -1, -1, -1, 0, 1};
    int neighbor8_offset_y[] = {0, -1, -1, -1, 0, 1, 1, 1};
    int neighbor8_offset_end_idx, neighbor8_offset_idx;
    int neighbor_pixel;
    bool foundNonzeroNeighbor;
    
    ContourInformation contour_info;
    std::vector<ContourInformation> contour_infos;
    ContourInformation frame;
    frame.NBD = NBD;
    frame.is_hole_border = 1;
    frame.nxt_contour = -1;
    frame.prev_contour = -1;
    frame.first_child_contour = -1;
    frame.parent_contour = -1;
    contour_infos.push_back(frame);
    
    for (i = 1; i < height; i++) {
        LNBD = 1;
        for (j = 1; j < width; j++) {
            cur_pixel = image.ptr<int>(i)[j];
            prev_pixel = image.ptr<int>(i)[j-1];
            nxt_pixel = image.ptr<int>(i)[j+1];
            if (!((prev_pixel == 0 && cur_pixel == 1) || (cur_pixel >= 1 && nxt_pixel == 0))) {
                goto STEP_4;
            }
            is_hole_border = !(prev_pixel == 0 && cur_pixel == 1);
            
            NBD += 1;
            contour_info.contour.clear();
            contour_info.NBD = NBD;
            contour_info.is_hole_border = is_hole_border;
            contour_info.nxt_contour = contour_info.prev_contour = contour_info.first_child_contour = contour_info.parent_contour = -1;
            
            if (is_hole_border) {
                i2 = i;
                j2 = j+1;
                if (cur_pixel > 1) {
                    LNBD = cur_pixel;
                }
            } else {
                i2 = i;
                j2 = j-1;
            }
            
            for (c = 0; c < contour_infos.size(); c++) {
                if (contour_infos[c].NBD == LNBD) {
                    if (contour_infos[c].is_hole_border == is_hole_border) {
                        contour_info.parent_contour = contour_infos[c].parent_contour;
                        while (contour_infos[c].nxt_contour != -1) {
                            c = contour_infos[c].nxt_contour;
                        }
                        contour_infos[c].nxt_contour = contour_infos.size();
                        contour_info.prev_contour = c;
                    } else {
                        contour_info.parent_contour = c;
                        if (contour_infos[c].first_child_contour == -1) {
                            contour_infos[c].first_child_contour = contour_infos.size();
                        } else {
                            c = contour_infos[c].first_child_contour;
                            while (contour_infos[c].nxt_contour != -1) {
                                c = contour_infos[c].nxt_contour;
                            }
                            contour_infos[c].nxt_contour = contour_infos.size();
                            contour_info.prev_contour = c;
                        }
                    }
                    break;
                }
            }
            
            if (is_hole_border) {
                neighbor8_offset_idx = 0;
                neighbor8_offset_end_idx = 1;
            } else {
                neighbor8_offset_idx = 4;
                neighbor8_offset_end_idx = 5;
            }
            foundNonzeroNeighbor = false;
            while (true) {
                neighbor_pixel = image.ptr<int>(i+neighbor8_offset_y[neighbor8_offset_idx])[j+neighbor8_offset_x[neighbor8_offset_idx]];
                if (neighbor_pixel != 0) {
                    foundNonzeroNeighbor = true;
                    i1 = i + neighbor8_offset_y[neighbor8_offset_idx];
                    j1 = j + neighbor8_offset_x[neighbor8_offset_idx];
                    break;
                }
                if (neighbor8_offset_idx == neighbor8_offset_end_idx) {
                    break;
                }
                neighbor8_offset_idx = (neighbor8_offset_idx-1) & 7;
            }
            if (!foundNonzeroNeighbor) {
                image.ptr<int>(i)[j] = -NBD;
                contour_info.contour.push_back(cv::Point(j-1, i-1));
                contour_infos.push_back(contour_info);
                goto STEP_4;
            }
            
            i2 = i1;
            j2 = j1;
            i3 = i;
            j3 = j;
            neighbor8_offset_end_idx = neighbor8_offset_idx;
            neighbor8_offset_idx = (neighbor8_offset_idx+1) & 7;
            while (true) {
                contour_info.contour.push_back(cv::Point(j3-1, i3-1));
                foundNonzeroNeighbor = false;
                while (true) {
                    neighbor_pixel = image.ptr<int>(i3+neighbor8_offset_y[neighbor8_offset_idx])[j3+neighbor8_offset_x[neighbor8_offset_idx]];
                    if (neighbor_pixel != 0) {
                        foundNonzeroNeighbor = true;
                        i4 = i3 + neighbor8_offset_y[neighbor8_offset_idx];
                        j4 = j3 + neighbor8_offset_x[neighbor8_offset_idx];
                        break;
                    }
                    if (neighbor8_offset_idx == neighbor8_offset_end_idx) {
                        break;
                    }
                    neighbor8_offset_idx = (neighbor8_offset_idx+1) & 7;
                }
                
                nxt_pixel = image.ptr<int>(i3)[j3+1];
                if (nxt_pixel == 0) {
                    image.ptr<int>(i3)[j3] = -NBD;
                } else {
                    tmp_pixel = image.ptr<int>(i3)[j3];
                    if (tmp_pixel == 1) {
                        image.ptr<int>(i3)[j3] = NBD;
                    }
                }
                
                if (i4 == i && j4 == j && i3 == i1 && j3 == j1) {
                    contour_infos.push_back(contour_info);
                    goto STEP_4;
                } else {
                    i2 = i3;
                    j2 = j3;
                    i3 = i4;
                    j3 = j4;
                }
                neighbor8_offset_end_idx = (neighbor8_offset_idx + 4) & 7;
                neighbor8_offset_idx = (neighbor8_offset_end_idx + 1) & 7;
            }
            
            STEP_4:
            {
                if (cur_pixel != 1 && cur_pixel != 0) {
                    LNBD = (cur_pixel < 0 ? -cur_pixel : cur_pixel);
                }
            }
        }
    }
    
    contours.clear();
    hierarchy.clear();
    for (c = 1; c < contour_infos.size(); c++) {
        contours.push_back(contour_infos[c].contour);
        int nxt = (contour_infos[c].nxt_contour == -1 ? -1 : contour_infos[c].nxt_contour-1);
        int prev = (contour_infos[c].prev_contour == -1 ? -1 : contour_infos[c].prev_contour-1);
        int first = (contour_infos[c].first_child_contour == -1 ? -1 : contour_infos[c].first_child_contour-1);
        int parent = (contour_infos[c].parent_contour == -1 ? -1 : contour_infos[c].parent_contour-1);
        hierarchy.push_back(cv::Vec4i(nxt, prev, first, parent));
    }
}

/*
int main(int argc, char** argv) {
    cv::Mat src = cv::Mat::zeros(400, 400, CV_8UC1);
    cv::rectangle(src, cv::Rect(20, 20, 100, 20), cv::Scalar(255), cv::FILLED, cv::LINE_8);
    cv::rectangle(src, cv::Rect(150, 30, 230, 20), cv::Scalar(255), cv::FILLED, cv::LINE_8);
    cv::rectangle(src, cv::Rect(20, 80, 360, 300), cv::Scalar(255), cv::FILLED, cv::LINE_8);
    cv::rectangle(src, cv::Rect(40, 100, 320, 260), cv::Scalar(0), cv::FILLED, cv::LINE_8);
    cv::rectangle(src, cv::Rect(70, 130, 260, 200), cv::Scalar(255), cv::FILLED, cv::LINE_8);
    cv::rectangle(src, cv::Rect(90, 150, 220, 160), cv::Scalar(0), cv::FILLED, cv::LINE_8);
    cv::rectangle(src, cv::Rect(110, 170, 60, 120), cv::Scalar(255), cv::FILLED, cv::LINE_8);
    cv::rectangle(src, cv::Rect(190, 190, 100, 90), cv::Scalar(255), cv::FILLED, cv::LINE_8);
    
    std::vector<std::vector<cv::Point>> contours;
    std::vector<cv::Vec4i> hierarchy;*/
    //cv::findContours(src, contours, hierarchy, cv::RETR_EXTERNAL, cv::CHAIN_APPROX_NONE, cv::Point(0, 0));
    //cv::findContours(src, contours, hierarchy, cv::RETR_LIST, cv::CHAIN_APPROX_NONE, cv::Point(0, 0));
    //cv::findContours(src, contours, hierarchy, cv::RETR_CCOMP, cv::CHAIN_APPROX_NONE, cv::Point(0, 0));
    //cv::findContours(src, contours, hierarchy, cv::RETR_TREE, cv::CHAIN_APPROX_NONE, cv::Point(0, 0));
    //cv::findContours(src, contours, hierarchy, cv::RETR_TREE, cv::CHAIN_APPROX_NONE, cv::Point(20, 20));
    //cv::findContours(src, contours, hierarchy, cv::RETR_TREE, cv::CHAIN_APPROX_NONE, cv::Point(-20, -20));
    /*cv::findContours(src, contours, hierarchy, cv::RETR_TREE, cv::CHAIN_APPROX_NONE, cv::Point(-50, -20));
    std::cout << "contours size = " << contours.size() << std::endl;
    std::cout << "hierarchy size = " << hierarchy.size() << std::endl;
    std::cout << "============================================================" << std::endl;
    */
    /*
    _find_contours(src, contours, hierarchy);
    
    cv::Mat src3c = cv::Mat::zeros(400, 400, CV_8UC3);
    cv::rectangle(src3c, cv::Rect(20, 20, 100, 20), cv::Scalar::all(255), cv::FILLED, cv::LINE_8);
    cv::rectangle(src3c, cv::Rect(150, 30, 230, 20), cv::Scalar::all(255), cv::FILLED, cv::LINE_8);
    cv::rectangle(src3c, cv::Rect(20, 80, 360, 300), cv::Scalar::all(255), cv::FILLED, cv::LINE_8);
    cv::rectangle(src3c, cv::Rect(40, 100, 320, 260), cv::Scalar::all(0), cv::FILLED, cv::LINE_8);
    cv::rectangle(src3c, cv::Rect(70, 130, 260, 200), cv::Scalar::all(255), cv::FILLED, cv::LINE_8);
    cv::rectangle(src3c, cv::Rect(90, 150, 220, 160), cv::Scalar::all(0), cv::FILLED, cv::LINE_8);
    cv::rectangle(src3c, cv::Rect(110, 170, 60, 120), cv::Scalar::all(255), cv::FILLED, cv::LINE_8);
    cv::rectangle(src3c, cv::Rect(190, 190, 100, 90), cv::Scalar::all(255), cv::FILLED, cv::LINE_8);
    
    cv::Scalar colors[] = {cv::Scalar(0, 0, 255), cv::Scalar(0, 255, 0), cv::Scalar(255, 0, 0),
                        cv::Scalar(255, 128, 0), cv::Scalar(128, 0, 255), cv::Scalar(0, 128, 255),
                        cv::Scalar(128, 0, 255), cv::Scalar(0, 255, 128), cv::Scalar(255, 0, 128)};
    for (int i = 0; i < contours.size(); i++) {
        std::cout << "number of points of contour " << i << " = " << contours[i].size() << std::endl;
        std::cout << "current contour is " << i << std::endl;
        std::cout << "next contour is " << hierarchy[i][0] << std::endl;
        std::cout << "previous contour is " << hierarchy[i][1] << std::endl;
        std::cout << "first child contour is " << hierarchy[i][2] << std::endl;
        std::cout << "parent contour is " << hierarchy[i][3] << std::endl;
        for (int j = 0; j < contours[i].size(); j++) {
            cv::rectangle(src3c, cv::Rect(contours[i][j].x-2, contours[i][j].y-2, 5, 5), colors[i], cv::FILLED, cv::LINE_8);
            cv::putText(src3c, cv::format("%d", i), cv::Point(contours[i][0].x+5, contours[i][0].y+15), 1, 1, colors[i], 1, cv::LINE_8, false);
        }
        std::cout << "============================================================" << std::endl;
    }
    
    int colorIdx = 0;
    int maxLevel = 4;
    //for (int i = 0; i >= 0; i = hierarchy[i][0]) {
     //   cv::drawContours(src3c, contours, i, colors[colorIdx++], 5, cv::LINE_8, hierarchy, maxLevel, cv::Point());
    //}
    
    cv::imshow("src", src);
    cv::imshow("src3c", src3c);
    cv::waitKey(0);
    */
    
    /*cv::Mat src = cv::Mat::zeros(400, 400, CV_8UC1);
    cv::rectangle(src, cv::Rect(100, 100, 200, 200), cv::Scalar(255), cv::FILLED, cv::LINE_8);
    
    std::vector<std::vector<cv::Point>> contours_none, contours_simple, contours_tc89_l1, contours_tc89_kcos;
    std::vector<cv::Vec4i> hierarchy_none, hierarchy_simple, hierarchy_tc89_l1, hierarchy_tc89_kcos;
    cv::findContours(src, contours_none, hierarchy_none, cv::RETR_LIST, cv::CHAIN_APPROX_NONE, cv::Point(0,0));
    cv::findContours(src, contours_simple, hierarchy_simple, cv::RETR_LIST, cv::CHAIN_APPROX_SIMPLE, cv::Point(0,0));
    cv::findContours(src, contours_tc89_l1, hierarchy_tc89_l1, cv::RETR_LIST, cv::CHAIN_APPROX_TC89_L1, cv::Point(0,0));
    cv::findContours(src, contours_tc89_kcos, hierarchy_tc89_kcos, cv::RETR_LIST, cv::CHAIN_APPROX_TC89_KCOS, cv::Point(0,0));
    
    cv::Mat mat_none = cv::Mat::zeros(400, 400, CV_8UC3);
    cv::Mat mat_simple = cv::Mat::zeros(400, 400, CV_8UC3);
    cv::Mat mat_tc89_l1 = cv::Mat::zeros(400, 400, CV_8UC3);
    cv::Mat mat_tc89_kcos = cv::Mat::zeros(400, 400, CV_8UC3);
    cv::rectangle(mat_none, cv::Rect(100, 100, 200, 200), cv::Scalar(255, 255, 255), cv::FILLED, cv::LINE_8);
    cv::rectangle(mat_simple, cv::Rect(100, 100, 200, 200), cv::Scalar(255, 255, 255), cv::FILLED, cv::LINE_8);
    cv::rectangle(mat_tc89_l1, cv::Rect(100, 100, 200, 200), cv::Scalar(255, 255, 255), cv::FILLED, cv::LINE_8);
    cv::rectangle(mat_tc89_kcos, cv::Rect(100, 100, 200, 200), cv::Scalar(255, 255, 255), cv::FILLED, cv::LINE_8);
    
    for (int i = 0; i < contours_none.size(); i++) {
        std::cout << "none size = " << contours_none[i].size() << "--" << i << std::endl;
        for (int j = 0; j < contours_none[i].size(); j++) {
            cv::rectangle(mat_none, cv::Rect(contours_none[i][j].x - 4, contours_none[i][j].y - 4, 9, 9), cv::Scalar(0, 0, 255), cv::FILLED, cv::LINE_8);
        }
    }
    for (int i = 0; i < contours_simple.size(); i++) {
        std::cout << "simle size = " << contours_simple[i].size() << "--" << i << std::endl;
        for (int j = 0; j < contours_simple[i].size(); j++) {
            cv::rectangle(mat_simple, cv::Rect(contours_simple[i][j].x - 4, contours_simple[i][j].y - 4, 9, 9), cv::Scalar(0, 0, 255), cv::FILLED, cv::LINE_8);
        }
    }
    for (int i = 0; i < contours_tc89_l1.size(); i++) {
        std::cout << "tc89_l1 size = " << contours_tc89_l1[i].size() << "--" << i << std::endl;
        for (int j = 0; j < contours_tc89_l1[i].size(); j++) {
            cv::rectangle(mat_tc89_l1, cv::Rect(contours_tc89_l1[i][j].x - 4, contours_tc89_l1[i][j].y - 4, 9, 9), cv::Scalar(0, 0, 255), cv::FILLED, cv::LINE_8);
        }
    }
    for (int i = 0; i < contours_tc89_kcos.size(); i++) {
        std::cout << "tc89_kcos size = " << contours_tc89_kcos[i].size() << "--" << i << std::endl;
        for (int j = 0; j < contours_tc89_kcos[i].size(); j++) {
            cv::rectangle(mat_tc89_kcos, cv::Rect(contours_tc89_kcos[i][j].x - 4, contours_tc89_kcos[i][j].y - 4, 9, 9), cv::Scalar(0, 0, 255), cv::FILLED, cv::LINE_8);
        }
    }
    
    cv::imshow("src", src);
    cv::imshow("none", mat_none);
    cv::imshow("simple", mat_simple);
    cv::imshow("tc89_l1", mat_tc89_l1);
    cv::imshow("tc89_kcos", mat_tc89_kcos);
    cv::waitKey(0);
    */
    //return EXIT_SUCCESS;
//}


int main(int argc, char **argv) {
    //cv::Mat src = cv::imread("/home/xlll/Downloads/opencv/samples/data/HappyFish.jpg", cv::IMREAD_GRAYSCALE);
    cv::Mat src = cv::imread("/home/xlll/Downloads/opencv/samples/data/lena.jpg", cv::IMREAD_GRAYSCALE);
    if (src.empty()) {
        std::cout << "failed to read image" << std::endl;
        return EXIT_FAILURE;
    }
    
    cv::Mat matBlur;
    cv::blur(src, matBlur, cv::Size(3, 3));
    cv::Mat matCanny;
    cv::Canny(matBlur, matCanny, 100, 100*2);
    
    std::vector<std::vector<cv::Point>> contours;
    std::vector<cv::Vec4i> hierarchy;
    cv::findContours(matCanny, contours, hierarchy, cv::RETR_LIST, cv::CHAIN_APPROX_SIMPLE, cv::Point(0, 0));
    cv::Mat matContours = cv::Mat::zeros(matCanny.size(), CV_8UC3);
    cv::RNG rng(100);
    for (int i = 0; i < contours.size(); i++) {
        cv::Scalar color = cv::Scalar(rng.uniform(0, 255), rng.uniform(0, 255), rng.uniform(0, 255));
        cv::drawContours(matContours, contours, i, color, 1, 8, hierarchy, 0, cv::Point());
    }
    std::cout << "number of contours = " << contours.size() << std::endl;
    
    std::vector<std::vector<cv::Point>> contours1;
    std::vector<cv::Vec4i> hierarchy1;
    _find_contours(matCanny, contours1, hierarchy1);
    cv::Mat matContours1 = cv::Mat::zeros(matCanny.size(), CV_8UC3);
    for (int i = 0; i < contours1.size(); i++) {
        cv::Scalar color = cv::Scalar(rng.uniform(0, 255), rng.uniform(0, 255), rng.uniform(0, 255));
        for (int j = 0; j < contours1[i].size()-1; j++) {
            cv::line(matContours1, contours1[i][j], contours1[i][j+1], color, 1, cv::LINE_8);
        }
    }
    std::cout << "number of contours1 = " << contours1.size() << std::endl;
    
    cv::imshow("src", src);
    cv::imshow("matBlur", matBlur);
    cv::imshow("matCanny", matCanny);
    cv::imshow("matContours", matContours);
    cv::imshow("matContours1", matContours1);
    cv::waitKey(0);
    
    return 0;
}

